package org.moka.ioio.ioiopilot;

import android.app.Activity;
import android.hardware.Sensor;
import android.hardware.SensorEvent;
import android.hardware.SensorEventListener;
import android.hardware.SensorManager;
import android.os.Bundle;
import android.util.Log;

public class SensorActivity extends Activity implements SensorEventListener {

	private  SensorManager mSensorManager;
	private  Sensor mAccelerometer;
	private Sensor sMagnetic;
	
	private boolean calibrated = false;

	float[] inR = new float[16];
	
	float[] accArray = new float[3];
	float[] accRawArray = new float[3];
	float[] accZeroArray = new float[3];
	float[] accPrev = new float[3];
	float[] gyroArray = new float[3];
	
	float[] oriArray = new float[3];
	float[] oriRawArray = new float[3];
	float[] oriZeroArray = new float[3];
	
	float[] magRawArray = new float[3];
	
	private float[] gravityAcc = new float[3];
	private float[] linearAcc = new float[3];
	
	final float pi = (float) Math.PI;
	final float rad2deg = 180/pi;
	
	private int count = 0;
	private long time;
	
	
	
	  


	public void onCreate(Bundle savedInstanceState) {
		super.onCreate(savedInstanceState);
		mSensorManager = (SensorManager)getSystemService(SENSOR_SERVICE);
		;
		Log.d("SensorActivity", "Sensors");
		for(Sensor s : mSensorManager.getSensorList(Sensor.TYPE_ALL))
		{
			Log.d("SensorActivity", "Sensor: "+ s.getName()+" T:"+s.getType()+" vendor:"+s.getVendor());
		}
		mAccelerometer = mSensorManager.getDefaultSensor(Sensor.TYPE_ACCELEROMETER);
		//sMagnetic = mSensorManager.getDefaultSensor(Sensor.TYPE_MAGNETIC_FIELD);
	}

	protected void onResume() {
		super.onResume();
		mSensorManager.registerListener(this, mAccelerometer, SensorManager.SENSOR_DELAY_GAME);
		//mSensorManager.registerListener(this, sMagnetic, SensorManager.SENSOR_DELAY_NORMAL);
	}

	protected void onPause() {
		super.onPause();
		mSensorManager.unregisterListener(this);
	}

	public void onAccuracyChanged(Sensor sensor, int accuracy) {
	}

	public void onSensorChanged(SensorEvent event) {
		switch (event.sensor.getType()){  
		case Sensor.TYPE_ACCELEROMETER:
			accRawArray = event.values.clone();
			accArray[0] = event.values[0]-accZeroArray[0];
			accArray[1] = event.values[1]-accZeroArray[1];
			accArray[2] = event.values[2]-accZeroArray[2];
			
			
			// alpha is calculated as t / (t + dT)
	          // with t, the low-pass filter's time-constant
	          // and dT, the event delivery rate

	          final float alpha = (float) 0.8;
	          gravityAcc[0] = alpha * gravityAcc[0] + (1 - alpha) * event.values[0];
	          gravityAcc[1] = alpha * gravityAcc[1] + (1 - alpha) * event.values[1];
	          gravityAcc[2] = alpha * gravityAcc[2] + (1 - alpha) * event.values[2];

	          linearAcc[0] = event.values[0] - gravityAcc[0];
	          linearAcc[1] = event.values[1] - gravityAcc[1];
	          linearAcc[2] = event.values[2] - gravityAcc[2];
	          
	          calculateGyro();
			
	          
	          
			break;
		case Sensor.TYPE_MAGNETIC_FIELD:
			magRawArray = event.values.clone();
			calculateOrientation();
			break;
		}
		//Log.d("SensorActivity", "x:"+((int)(event.values[0]/9.8*500))+" y:"+event.values[1]+" z:"+event.values[2]);

		//Log.d("Magnetic", "x:"+(event.values[0])+" y:"+event.values[1]+" z:"+event.values[2]);
		   
	}

	public void calculateGyro()
	{
		if(time>0)
		{
			float dX = accArray[0]-accPrev[0];
			float dY = accArray[1]-accPrev[1];
			float dZ = accArray[2]-accPrev[2];
			float dT = System.nanoTime() - time;
			gyroArray[0] = (gyroArray[0] + dX*1000000000/dT)/2;
			gyroArray[1] = (gyroArray[1] + dY*1000000000/dT)/2;
			gyroArray[2] = (gyroArray[2] + dZ*1000000000/dT)/2;
			Log.d("SensorActivity",					
					"GYRO X: " + gyroArray[0] + " m/s^2 " + 
					"Y: " + gyroArray[1] + " m/s^2 " + 
					"Z: " + gyroArray[2] + " m/s^2 "+
					"dX: "+dX+ " dT: "+dT);
		}
		time = System.nanoTime();		
		accPrev[0] = accArray[0];
		
	}
	
	private void calculateOrientation() {
		if (accRawArray != null && magRawArray != null)
		{

			// checks that the rotation matrix is found
			boolean success = SensorManager.getRotationMatrix(inR, null, accRawArray, magRawArray);
			if (success){

				// Finds the Azimuth and Pitch angles of the y-axis with  
				// magnetic north and the horizon respectively
				SensorManager.getOrientation(inR, oriRawArray);
				
				if(!calibrated)calibrate();
				
				oriArray[0] = oriRawArray[0]-oriZeroArray[0];
				if(oriArray[0]<-Math.PI)oriArray[0] = (oriArray[0] + 2*pi);
				else if(oriArray[0]>Math.PI)oriArray[0] = (oriArray[0] - 2*pi);
				
				oriArray[1] = oriRawArray[1]-oriZeroArray[1];
				if(oriArray[1]<-Math.PI)oriArray[1] = (oriArray[1] + 2*pi);
				else if(oriArray[1]>Math.PI)oriArray[1] = (oriArray[1] - 2*pi);
				
				oriArray[2] = oriRawArray[2]-oriZeroArray[2];
				if(oriArray[2]<-Math.PI)oriArray[2] = (oriArray[2] + 2*pi);
				else if(oriArray[2]>Math.PI)oriArray[2] = (oriArray[2] - 2*pi);
				
				if(count>20) 
				{
					
					Log.d("SensorActivity",					
						"Roll: " + oriArray[2] + " rad "+ 
						"Pitch: " + oriArray[1] + " rad " + 
						"Azimuth: " + oriArray[0] + " rad " );
					Log.d("SensorActivity",					
							"X: " + accArray[0] + " m/s^2 " + 
							"Y: " + accArray[1] + " m/s^2 " + 
							"Z: " + accArray[2] + " m/s^2 ");
					Log.d("SensorActivity",					
							"XL: " + linearAcc[0] + " m/s^2 " + 
							"YL: " + linearAcc[1] + " m/s^2 " + 
							"ZL: " + linearAcc[2] + " m/s^2 ");
					count=0;
					
				}
				count++;
			} 
		}
	}

	private void calibrate() {
		accZeroArray = accRawArray.clone();
		oriZeroArray = oriRawArray.clone();
		Log.d("SensorActivity","CALIBRATED");
		calibrated = true;
	}
}

